package lego.jbv;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.LightSensor;
import lejos.nxt.SensorPort;
import lejos.nxt.addon.GyroSensor;
import lejos.util.Delay;

public class main {

	public static void main(String[] args) {				
		LCD.drawString("Ready", 0, 0);
		Button.waitForAnyPress();
		LCD.clear();
		
		Cruise cruise = new Cruise();
		Line line = new Line();
		
		line.calibrate();
		
		ICommand[] workers = {line, cruise};
		
		Arbiter arbiter = new Arbiter(workers);
		
		//arbiter.start();
		//cruise.start();
		//line.start();
		
		//test code
		GyroSensor gyro = new GyroSensor(SensorPort.S2);
		gyro.recalibrateOffset();
		LCD.clear();
		LCD.drawString("Gyro raw:", 0, 0);
		LCD.drawString("Gyro AV:", 0, 1);
		//End of testcode
		
		while(Button.ESCAPE.isUp())
		{
			//TestCode
			LCD.drawInt(gyro.readValue(), 4, 11, 0);
			LCD.drawInt((int)gyro.getAngularVelocity(), 4, 11, 1);
			//EndOfTestCode
			
			Delay.msDelay(100);
		}
		
		System.exit(0);
		cruise.stop();
		line.stop();
	}

}
